/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CLineMapRawScanMCL.h"

CLineMapRawScanMCL::CLineMapRawScanMCL(){
		
		num_of_meas = 0;
		sensor_offset = 0;
		virtualr = NULL;
		virtuallines = NULL;
		virtualscan_xy = NULL;
		virtualscan_xy_rel = NULL;
}
CLineMapRawScanMCL::~CLineMapRawScanMCL(){
}

/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
 */
void CLineMapRawScanMCL::initialize(const char *mapfile,
								float mapscale, 
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								int num_of_meas_in_scan,
								float _sensor_offset){
										
		initializeVariables(mapfile,mapscale,num_of_meas_in_scan, _sensor_offset);
				
		filt.initializeNormalRandom(initPose, initVariance,num_of_particles);													
}

/**
 * Initializes the filter - uses uniform distribution method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * The initial distribution is calculated using bounding box of the map. The @dP sets the 
 * resolution of the unform grid.  WARNING the initialization might not be 
 * real-time.
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p dP the resolution for the uniform grid
 * @p num_of_meas_in_scan number of measurements used from the scan
 */
void CLineMapRawScanMCL::initializeGlobal(const char *mapfile,
															float mapscale,  
															mcl::pose dP, 
															int num_of_meas_in_scan,
															float _sensor_offset)
{
		TLine bb;
		mcl::pose start,end;
		
		initializeVariables(mapfile,mapscale,num_of_meas_in_scan, _sensor_offset);
		
		bb = map.getMapBoundingBox();
		
		fprintf(stderr,"Got BB:(%.2f,%.2f),(%.2f,%.2f) ",bb.x1,bb.y1,bb.x2,bb.y2);
		start.set(bb.x1,bb.y1,0);
		end.set(bb.x2,bb.y2,2*M_PI);
		filt.initializeUniform(start, end,dP);
}							

/**
 * Initialises the variables
*/
void CLineMapRawScanMCL::initializeVariables(const char *mapfile,
															float mapscale,  
															int num_of_meas_in_scan,
															float _sensor_offset)
{
		if(map.readMapFile(mapfile,mapscale)!=0){
				fprintf(stderr,"CLineMapRawScanMCL::initializeVariables::Unable to load the map\n");
				exit(1);
		}
		
		fprintf(stderr,"CLineMapRawScanMCL::initializeVariables(): Called with num_of_meas_in_scan=%d\n",num_of_meas_in_scan);
		
		/// Initialize memory for the private members
		virtualr = (float *) malloc(sizeof(float)*(num_of_meas_in_scan));
		virtuallines= (float *) malloc(3*sizeof(float)*(num_of_meas_in_scan));
		virtualscan_xy = (float *) malloc(2*sizeof(float)*(num_of_meas_in_scan));
		virtualscan_xy_rel = (float *) malloc(2*sizeof(float)*(num_of_meas_in_scan));
		
		num_of_meas  =	num_of_meas_in_scan;
		sensor_offset = _sensor_offset;
}
/**
 * Same as init but without mallocs
 */
void CLineMapRawScanMCL::reinitializeNR(mcl::pose initPose,mcl::pose initVariance){
		filt.initializeNormalRandom(initPose, initVariance,filt.NumOfParticles);	
}

/**
 * Run the MCL step
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CLineMapRawScanMCL::runMCL(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		int fac;
		mcl::pose var;
		static float sinceSIR=0; 
		//float sensor_offset_tmp;
		fac = measurement.N / num_of_meas; ///< The ratio between the measurement and wanted num_of_meas
		
		mcl_meas.allocate(num_of_meas);
		mcl_meas.N=num_of_meas;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
		for(int i=0;i<num_of_meas;i++){
			//fprintf(stderr,"i=%d faci=%d ",i,fac*i);
				mcl_meas.r[i] = measurement.r[fac*i];	
				mcl_meas.a[i] = measurement.a[fac*i];
			if(fac*i>=measurement.N) 
					fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
		}
		//float Q[4];
		//Q[0] = variance.x;
		//Q[1] = 0.01;
		//Q[2] = variance.y;
		//Q[3] = variance.a;
		//filt.predict(dP,Q); ///<Predict
		
		filt.predict(dP,variance); ///<Predict
		/*
		sensor_offset_tmp = sensor_offset;
		for(int jj=0;jj<4;jj++){		 
				calculateLikelihood(mcl_meas); ///<Calculate likelihoods
				fprintf(stderr,"Lik with %.3f offset = %.3f\n",sensor_offset,filt.Lik);
				sensor_offset += M_PI/2;
		}
		sensor_offset = sensor_offset_tmp;
		*/
		calculateLikelihood(mcl_meas); ///<Calculate likelihoods
		
		filt.normalize(); 
		var = filt.getDistributionVariances(); 
		float rad = sqrt(var.x*var.x + var.y*var.y + var.a*var.a);
		
		///Calculate the variance of the probabilities
		float varP=0;
		float iN = 1.0 / (float) filt.NumOfParticles;
		for(int i = 0; i<filt.NumOfParticles;i++){
				varP += (filt.Particles[i].p - iN)*(filt.Particles[i].p - iN);
		}
		
		///Using now stetson harrison method
		//if(var.x > .1 || var.y>.1 || var.a > (M_PI/16)){
		//if(rad > .1 || sinceSIR > 5){
		
		if(varP > .1 || sinceSIR > 25){
				//fprintf(stderr,"Performing SIR varP=%.3f\n",varP);
				filt.SIRUpdate();
				sinceSIR = 0;
		}
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}

/**
 * Run the MCL step in initialization mode
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CLineMapRawScanMCL::runMCLInit(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		int fac;
		mcl::pose var;
		static int sinceSIR=2;
		
		
		fac = measurement.N / num_of_meas; ///< The ratio between the measurement and wanted num_of_meas
		
		mcl_meas.allocate(num_of_meas);
		mcl_meas.N=num_of_meas;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
		for(int i=0;i<num_of_meas;i++){
			//fprintf(stderr,"i=%d faci=%d ",i,fac*i);
				mcl_meas.r[i] = measurement.r[fac*i];	
				mcl_meas.a[i] = measurement.a[fac*i];
				if(fac*i>=measurement.N) 
						fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
		}
		
		filt.predict(dP,variance); ///<Predict
		calculateLikelihood(mcl_meas); ///<Calculate likelihoods
		filt.normalize(); 
		var = filt.getDistributionVariances(); 
		/*
		if(filt.NumOfParticles > 1000 && sinceSIR==1){
				filt.resize(filt.NumOfParticles*0.5);
				filt.normalize();
		}
		
		*/
		filt.SIRUpdate(); ///< allways update!
		
		///Using now stetson harrison method
		//if((var.x > 1.0 || var.y>1.0 || var.a > (M_PI/4))&&sinceSIR>0){
		//		fprintf(stderr,"Performing SIR \n");
		//		
		//		sinceSIR = 0;
		//}
		
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}


/**
 * Calculates the likelihood of particle distribution
 * uses the CParticleFilter filt and TLineMap map
 * @param meas The current measurement
 * Uses the private members:
				float *virtualr;
				float *virtuallines;
				float *virtualscan_xy;
				float *virtualscan_xy_rel;
				int num_of_meas;
 **/
void CLineMapRawScanMCL::calculateLikelihood(mcl::scan meas)
{
		int i,j;
		
		float dL2,dl;
		float Lik;
		float Liks;
		int outLiers;
		
		filt.Lik = 0;
		filt.outLiers = 0;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		
		for(i=0;i<filt.NumOfParticles;i++){		
				outLiers = map.virtualScanner(virtualr, ///<out: distance "measurements"
																			virtualscan_xy_rel, ///< out: measurements in relative x,y-frame
																			virtualscan_xy, ///< out: measurements in absolute x,y-frame
																			virtuallines,  ///< out: The line parameters of the scan
																			filt.Particles[i].x,
																			filt.Particles[i].y,
																			filt.Particles[i].a,
																			num_of_meas, ///<Number of measurements taken
																			sensor_offset); ///<offset between laser and robot	meas.a[0] is added since the scan does not began allways from zero	
				
				filt.outLiers+=(float) outLiers;
		
				Liks = 1;
				
				for(j=0;j<num_of_meas;j++){	
						//r[j] = sqrt( Scan[2*j]* Scan[2*j] + Scan[2*j+1]*Scan[2*j+1]);
						//r_virt = sqrt(virtualScan[2*j]*virtualScan[2*j] + virtualScan[2*j+1]*virtualScan[2*j+1]);
									
						dl = meas.r[j] - virtualr[j];		// The difference between expected measurement and real measurement
		
						//If the value was ok
						if(meas.r[j]>=0){
				
								if(dl < -1.5f){
										Lik = 0.4f;
										Liks = Liks*Lik;
								}
								else{
										dL2 = dl*dl;
										Lik = (1.0- FALSE_PROB) * exp(  - (dL2/(VMEAS)) );
										//fprintf(stderr,"%.2f ",Lik);
										Lik += FALSE_PROB;
										Liks = Liks*Lik;
								}
						}
				}
				
				filt.Lik+=Liks;
				filt.Particles[i].lik=Liks;
		}
		
		filt.outLiers = (filt.outLiers / ( num_of_meas * filt.NumOfParticles))*100;

}
void CLineMapRawScanMCL::getScan(mcl::scan &meas, mcl::pose &p){
	float *vr = (float *) malloc(sizeof(float)*(meas.N));
	float *vlines= (float *) malloc(3*sizeof(float)*(meas.N));
	float *vscan_xy = (float *) malloc(2*sizeof(float)*(meas.N));
	float *vscan_xy_rel = (float *) malloc(2*sizeof(float)*(meas.N));
	
	map.virtualScanner(vr, ///<out: distance "measurements"
										 vscan_xy_rel, ///< out: measurements in relative x,y-frame
										 vscan_xy, ///< out: measurements in absolute x,y-frame
										 vlines,  ///< out: The line parameters of the scan
										 p.x,
										 p.y,
										 p.a,
										 meas.N, ///<Number of measurements taken
										 sensor_offset); ///<offset between laser and robot	
	float a=sensor_offset;
	float da=(M_PI)/meas.N;
	
	for(int i = 0;i<meas.N;i++){
		meas.r[i] = vr[i];
		meas.a[i] = a;
		a+=da;
	}
	free(vr);
	free(vlines);
	free(vscan_xy);
	free(vscan_xy_rel);
	
}
